/******************************************************************
 * This program was written as part of
 *   Assignment 4
 *   Computer Graphics                                             
 *   Semester 1, 2013
 *   School of Computer Science,
 *   The University of Adelaide
 *                                                                          
 * I declare that all material in this assessment is my own work except where
 * there is clear acknowledgement and reference to the work of others. I have
 * read the University Policy Statement on Plagiarism, Collusion and Related
 * Forms of Cheating:  

 * http://www.adelaide.edu.au/policies/?230

 * I give permission for my assessment work, including my subversion
 * repository, to be reproduced and submitted to academic staff for the
 * purposes of assessment and to be copied, submitted and retained in a form
 * suitable for electronic checking of plagiarism.  
                            
 * Authors:
 *  Terry Moschou    a1208955
 *  Salih Al-Rashid  a1210108
 *  Ben McAleer      a1212203
*******************************************************************/


#include "Segway.hpp"

#include "Model.hpp"

#include "Context.hpp"
#include "Scene.hpp"
#include "Terrain.hpp"

#include <math.h>

#define PI 3.14
#define GRAVY 0.05
#define YOFFSET 0.5

  const mat4 body_mat = glm::translate(mat4(1.0f), vec3(0.0f, 0.5f, 0.0f));
  const mat4 leftWheel_mat = glm::translate(mat4(1.0f), vec3(1.0f, 0.0, 0.0f));
  const mat4 rightWheel_mat = glm::translate(mat4(1.0f), vec3(-1.0f, 0.0f, 0.0f));

Segway::Segway(Object * parent /* = 0 */,
               const glm::mat4 & segwayModelMatrix /* = glm::mat4(1.0f) */)
  : Object(parent, segwayModelMatrix, 0, "Segway OBJ")
{
  Model * model = new Model("resources/models/segway.obj");
  m_body = new Object(this, mat4(1.0f), model, "Segway Model");
  model = new Model("resources/models/wheel.obj");
  m_leftWheel = new Object(this, leftWheel_mat, model, "Segway Left");
  m_rightWheel = new Object(this, rightWheel_mat, model, "Segway Right");
  m_grounded = true;
  m_mounted = false;
  m_vspeed = 0.0f;
  m_hspeed = 0.0f;
  m_rot = 0.0f;
  m_lspin = 0.0f;
  m_rspin = 0.0f;
}

void Segway::update() {
  g_context.m_scene->m_terrain->goforit = true;
  UIState &state = UserInterface::g_state;
 
  if(m_grounded) {
  if(m_hspeed > 0.005f)
    m_hspeed -= 0.005f;
  if(m_hspeed < 0.005f)
    m_hspeed += 0.005f;
  if(m_hspeed <= 0.01f && m_hspeed >= -0.01f)
    m_hspeed = 0.0f;
  }
  
  int dir_move = 0;
  int dir_angle = 0;
  
  if(m_mounted)
  {
  
  // get new player position
  if(state.m_w) dir_move = 1;
  if(state.m_s) dir_move = -1;
  if(state.m_d) dir_angle = -1;
  if(state.m_a) dir_angle = 1;
  if(state.m_space && m_grounded)
  { m_vspeed = 0.5f; m_grounded = false; }
  
  }
  
  if(m_grounded) m_rot += 5.0f*dir_angle;
  if (m_rot >= 360.0f)
    m_rot -= 360.0f;
  if (m_rot < 0.0f)
    m_rot += 360.0f;
  
  if(m_grounded)
    m_hspeed += (0.04f)*dir_move;
  else {
    m_lspin += 4.0f*dir_move;
    m_rspin += 4.0f*dir_move;
  }
  
  if(m_hspeed > 0.5f)
    m_hspeed = 0.5f;
  if(m_hspeed < -0.1f)
    m_hspeed = -0.1f;

  
  m_rspin += dir_angle*6.4f;
  m_lspin -= dir_angle*6.4f;
  
  vec3 posPrev = m_pos;
 
  m_pos.x += m_hspeed*sin(m_rot*PI/180);
  m_pos.z += m_hspeed*cos(m_rot*PI/180);
  
  m_rspin += m_hspeed*64.0;
  m_lspin += m_hspeed*64.0;
  if (m_lspin >= 360.0f)
    m_lspin -= 360.0f;
  if (m_lspin < 0.0f)
    m_lspin += 360.0f;
  if (m_rspin >= 360.0f)
    m_rspin -= 360.0f;
  if (m_rspin < 0.0f)
    m_rspin += 360.0f;

  float ground = YOFFSET+g_context.m_scene->m_terrain->getHeight(m_pos.x, m_pos.z);
  
  //Ouch?
  bool ouch = false;
  for(float ox = -1.0; ox < 1.1; ox = ox + 1.0)
    for(float oi = -0.6; oi < 3.1; oi = oi + 1.8)
      for(float oz = -1.0; oz < 1.1; oz = oz + 1.0)
        if(!g_context.boundcheck(m_pos+vec3(ox,oi,oz)))
          ouch = true;
  
  if(ouch) {
    m_vspeed = -GRAVY;
    m_grounded = false;
    m_pos = posPrev;
    m_hspeed = -m_hspeed;
    m_vspeed = 0.2f;
    ground = YOFFSET+g_context.m_scene->m_terrain->getHeight(m_pos.x, m_pos.z);
    m_grounded = false;
  }
  
  if(m_grounded)
    if(m_pos.y < ground+0.5)
      m_pos.y = ground;
    else {
      m_grounded = false;
      m_vspeed = 0.0f;
    }
  else {
    m_pos.y += m_vspeed;
    if(m_vspeed < 0.1 && m_vspeed > -0.05)
      m_vspeed -= GRAVY/8.0;
    else
      m_vspeed -= GRAVY;
    if(m_pos.y <= ground) {
      m_grounded = true;
      m_vspeed = 0.0f;
      m_pos.y = ground;
    }
  }
  
  mat4 segwayTransform = glm::rotate(glm::translate(mat4(1.0f), m_pos), m_rot, vec3(0.0f, 1.0f, 0.0f));
  setModelMatrix(segwayTransform);
  mat4 wheelTransform = glm::rotate(leftWheel_mat, m_lspin, vec3(1.0f, 0.0f, 0.0f));
  m_leftWheel->setModelMatrix(wheelTransform);
  wheelTransform = glm::rotate(rightWheel_mat, m_rspin, vec3(1.0f, 0.0f, 0.0f));
  m_rightWheel->setModelMatrix(wheelTransform);
  
  Object::update();
  g_context.m_scene->m_terrain->goforit = false;
}
